package wblut.geom;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Iterator;
import java.util.List;

import wblut.math.WB_Math;

/**
 * Represents a point on the two-dimensional surface of a globe. Latitude is the
 * degrees North and ranges between [-90, 90], while longitude refers to degrees
 * East, and ranges between (-180, 180].
 * <p/>
 * Instances of <code>WB_LatLon</code> are immutable.
 * 
 * @author Tom Gaskins
 * @version $Id$
 */
public class WB_LatLon {
	public static final WB_LatLon ZERO = new WB_LatLon(WB_Angle.ZERO,
			WB_Angle.ZERO);

	public enum Type {
		GREAT_CIRCLE, LOXODROME, RHUMB_LINE, LINEAR
	};

	public final WB_Angle latitude;
	public final WB_Angle longitude;

	/**
	 * Factor method for obtaining a new <code>WB_LatLon</code> from two angles
	 * expressed in radians.
	 * 
	 * @param latitude
	 *            in radians
	 * @param longitude
	 *            in radians
	 * 
	 * @return a new <code>WB_LatLon</code> from the given angles, which are
	 *         expressed as radians
	 */
	public static WB_LatLon fromRadians(double latitude, double longitude) {
		return new WB_LatLon(Math.toDegrees(latitude),
				Math.toDegrees(longitude));
	}

	/**
	 * Factory method for obtaining a new <code>WB_LatLon</code> from two angles
	 * expressed in degrees.
	 * 
	 * @param latitude
	 *            in degrees
	 * @param longitude
	 *            in degrees
	 * 
	 * @return a new <code>WB_LatLon</code> from the given angles, which are
	 *         expressed as degrees
	 */
	public static WB_LatLon fromDegrees(double latitude, double longitude) {
		return new WB_LatLon(latitude, longitude);
	}

	private WB_LatLon(double latitude, double longitude) {
		this.latitude = WB_Angle.fromDegrees(latitude);
		this.longitude = WB_Angle.fromDegrees(longitude);
	}

	/**
	 * Constructs a new <code>WB_LatLon</code> from two angles. Neither angle
	 * may be null.
	 * 
	 * @param latitude
	 *            latitude
	 * @param longitude
	 *            longitude
	 * 
	 * @throws IllegalArgumentException
	 *             if <code>latitude</code> or <code>longitude</code> is null
	 */
	public WB_LatLon(WB_Angle latitude, WB_Angle longitude) {
		if (latitude == null || longitude == null) {
			throw new IllegalArgumentException();
		}

		this.latitude = latitude;
		this.longitude = longitude;
	}

	public WB_LatLon(WB_LatLon latLon) {
		if (latLon == null) {

			throw new IllegalArgumentException();
		}

		this.latitude = latLon.latitude;
		this.longitude = latLon.longitude;
	}

	/**
	 * Obtains the latitude of this <code>WB_LatLon</code>.
	 * 
	 * @return this <code>WB_LatLon</code>'s latitude
	 */
	public final WB_Angle getLatitude() {
		return this.latitude;
	}

	/**
	 * Obtains the longitude of this <code>WB_LatLon</code>.
	 * 
	 * @return this <code>WB_LatLon</code>'s longitude
	 */
	public final WB_Angle getLongitude() {
		return this.longitude;
	}

	/**
	 * Returns an array of this object's latitude and longitude in degrees.
	 * 
	 * @return the array of latitude and longitude, arranged in that order.
	 */
	public double[] asDegreesArray() {
		return new double[] { this.getLatitude().degrees,
				this.getLongitude().degrees };
	}

	/**
	 * Returns an array of this object's latitude and longitude in radians.
	 * 
	 * @return the array of latitude and longitude, arranged in that order.
	 */
	public double[] asRadiansArray() {
		return new double[] { this.getLatitude().radians,
				this.getLongitude().radians };
	}

	/**
	 * Returns an interpolated location between <code>value1</code> and
	 * <code>value2</code>, according to the specified path type. If the path
	 * type is GREAT_CIRCLE this returns an interpolated value on the great arc
	 * that spans the two locations (see
	 * {@link #interpolateGreatCircle(double, WB_LatLon, WB_LatLon)}). If the
	 * path type is RHUMB_LINE or LOXODROME this returns an interpolated value
	 * on the rhumb line that spans the two locations (see
	 * {@link #interpolateRhumb(double, WB_LatLon, WB_LatLon)}. If LINEAR or
	 * otherwise, this returns the linear interpolation of the two locations
	 * (see {@link #interpolate(double, WB_LatLon, WB_LatLon)}.
	 * 
	 * @param pathType
	 *            the path type used to interpolate between geographic
	 *            locations.
	 * @param amount
	 *            the interpolation factor
	 * @param value1
	 *            the first location.
	 * @param value2
	 *            the second location.
	 * 
	 * @return an interpolated location between <code>value1</code> and
	 *         <code>value2</code>, according to the specified path type.
	 * 
	 * @throws IllegalArgumentException
	 *             if the path type or either location is null.
	 */
	public static WB_LatLon interpolate(String pathType, double amount,
			WB_LatLon value1, WB_LatLon value2) {
		if (pathType == null) {

			throw new IllegalArgumentException();
		}

		if (value1 == null || value2 == null) {

			throw new IllegalArgumentException();
		}

		if (pathType.equals(Type.GREAT_CIRCLE)) {
			return interpolateGreatCircle(amount, value1, value2);
		} else if (pathType.equals(Type.RHUMB_LINE)
				|| pathType.equals(Type.LOXODROME)) {
			return interpolateRhumb(amount, value1, value2);
		} else // Default to linear interpolation.
		{
			return interpolate(amount, value1, value2);
		}
	}

	/**
	 * Returns the linear interpolation of <code>value1</code> and
	 * <code>value2</code>, treating the geographic locations as simple 2D
	 * coordinate pairs.
	 * 
	 * @param amount
	 *            the interpolation factor
	 * @param value1
	 *            the first location.
	 * @param value2
	 *            the second location.
	 * 
	 * @return the linear interpolation of <code>value1</code> and
	 *         <code>value2</code>.
	 * 
	 * @throws IllegalArgumentException
	 *             if either location is null.
	 */
	public static WB_LatLon interpolate(double amount, WB_LatLon value1,
			WB_LatLon value2) {
		if (value1 == null || value2 == null) {

			throw new IllegalArgumentException();
		}

		if (WB_LatLon.equals(value1, value2))
			return value1;

		WB_Point v1 = new WB_Point(value1.getLongitude().radians,
				value1.getLatitude().radians, 0);
		WB_Point v2 = new WB_Point(value2.getLongitude().radians,
				value2.getLatitude().radians, 0);
		WB_Point p = WB_Point.interpolate(v1, v2, amount);

		return WB_LatLon.fromRadians(p.y, p.x);
	}

	/**
	 * Returns the an interpolated location along the great-arc between
	 * <code>value1</code> and <code>value2</code>. The interpolation factor
	 * <code>amount</code> defines the weight given to each value, and is
	 * clamped to the range [0, 1]. If <code>a</code> is 0 or less, this returns
	 * <code>value1</code>. If <code>amount</code> is 1 or more, this returns
	 * <code>value2</code>. Otherwise, this returns the location on the
	 * great-arc between <code>value1</code> and <code>value2</code>
	 * corresponding to the specified interpolation factor.
	 * 
	 * @param amount
	 *            the interpolation factor
	 * @param value1
	 *            the first location.
	 * @param value2
	 *            the second location.
	 * 
	 * @return an interpolated location along the great-arc between
	 *         <code>value1</code> and <code>value2</code>.
	 * 
	 * @throws IllegalArgumentException
	 *             if either location is null.
	 */
	public static WB_LatLon interpolateGreatCircle(double amount,
			WB_LatLon value1, WB_LatLon value2) {
		if (value1 == null || value2 == null) {

			throw new IllegalArgumentException();
		}

		if (WB_LatLon.equals(value1, value2))
			return value1;

		double t = WB_Math.clamp(amount, 0d, 1d);
		WB_Angle azimuth = WB_LatLon.greatCircleAzimuth(value1, value2);
		WB_Angle distance = WB_LatLon.greatCircleDistance(value1, value2);
		WB_Angle pathLength = WB_Angle.fromDegrees(t * distance.degrees);

		return WB_LatLon.greatCircleEndPosition(value1, azimuth, pathLength);
	}

	/**
	 * Returns the an interpolated location along the rhumb line between
	 * <code>value1</code> and <code>value2</code>. The interpolation factor
	 * <code>amount</code> defines the weight given to each value, and is
	 * clamped to the range [0, 1]. If <code>a</code> is 0 or less, this returns
	 * <code>value1</code>. If <code>amount</code> is 1 or more, this returns
	 * <code>value2</code>. Otherwise, this returns the location on the rhumb
	 * line between <code>value1</code> and <code>value2</code> corresponding to
	 * the specified interpolation factor.
	 * 
	 * @param amount
	 *            the interpolation factor
	 * @param value1
	 *            the first location.
	 * @param value2
	 *            the second location.
	 * 
	 * @return an interpolated location along the rhumb line between
	 *         <code>value1</code> and <code>value2</code>
	 * 
	 * @throws IllegalArgumentException
	 *             if either location is null.
	 */
	public static WB_LatLon interpolateRhumb(double amount, WB_LatLon value1,
			WB_LatLon value2) {
		if (value1 == null || value2 == null) {
			throw new IllegalArgumentException();
		}

		if (WB_LatLon.equals(value1, value2))
			return value1;

		double t = WB_Math.clamp(amount, 0d, 1d);
		WB_Angle azimuth = WB_LatLon.rhumbAzimuth(value1, value2);
		WB_Angle distance = WB_LatLon.rhumbDistance(value1, value2);
		WB_Angle pathLength = WB_Angle.fromDegrees(t * distance.degrees);

		return WB_LatLon.rhumbEndPosition(value1, azimuth, pathLength);
	}

	/**
	 * Computes the great circle angular distance between two locations. The
	 * return value gives the distance as the angle between the two positions on
	 * the pi radius circle. In radians, this angle is also the arc length of
	 * the segment between the two positions on that circle. To compute a
	 * distance in meters from this value, multiply it by the radius of the
	 * globe.
	 * 
	 * @param p1
	 *            WB_LatLon of the first location
	 * @param p2
	 *            WB_LatLon of the second location
	 * 
	 * @return the angular distance between the two locations. In radians, this
	 *         value is the arc length on the radius pi circle.
	 */
	public static WB_Angle greatCircleDistance(WB_LatLon p1, WB_LatLon p2) {
		if ((p1 == null) || (p2 == null)) {
			throw new IllegalArgumentException();
		}

		double lat1 = p1.getLatitude().radians;
		double lon1 = p1.getLongitude().radians;
		double lat2 = p2.getLatitude().radians;
		double lon2 = p2.getLongitude().radians;

		if (lat1 == lat2 && lon1 == lon2)
			return WB_Angle.ZERO;

		// "Haversine formula," taken from
		// http://en.wikipedia.org/wiki/Great-circle_distance#Formul.C3.A6
		double a = Math.sin((lat2 - lat1) / 2.0);
		double b = Math.sin((lon2 - lon1) / 2.0);
		double c = a * a + +Math.cos(lat1) * Math.cos(lat2) * b * b;
		double distanceRadians = 2.0 * Math.asin(Math.sqrt(c));

		return Double.isNaN(distanceRadians) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(distanceRadians);
	}

	/**
	 * Computes the azimuth angle (clockwise from North) that points from the
	 * first location to the second location. This angle can be used as the
	 * starting azimuth for a great circle arc that begins at the first
	 * location, and passes through the second location.
	 * 
	 * @param p1
	 *            WB_LatLon of the first location
	 * @param p2
	 *            WB_LatLon of the second location
	 * 
	 * @return WB_Angle that points from the first location to the second
	 *         location.
	 */
	public static WB_Angle greatCircleAzimuth(WB_LatLon p1, WB_LatLon p2) {
		if ((p1 == null) || (p2 == null)) {
			;
			throw new IllegalArgumentException();
		}

		double lat1 = p1.getLatitude().radians;
		double lon1 = p1.getLongitude().radians;
		double lat2 = p2.getLatitude().radians;
		double lon2 = p2.getLongitude().radians;

		if (lat1 == lat2 && lon1 == lon2)
			return WB_Angle.ZERO;

		if (lon1 == lon2)
			return lat1 > lat2 ? WB_Angle.POS180 : WB_Angle.ZERO;

		// Taken from "Map Projections - A Working Manual", page 30, equation
		// 5-4b.
		// The atan2() function is used in place of the traditional atan(y/x) to
		// simplify the case when x==0.
		double y = Math.cos(lat2) * Math.sin(lon2 - lon1);
		double x = Math.cos(lat1) * Math.sin(lat2) - Math.sin(lat1)
				* Math.cos(lat2) * Math.cos(lon2 - lon1);
		double azimuthRadians = Math.atan2(y, x);

		return Double.isNaN(azimuthRadians) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(azimuthRadians);
	}

	/**
	 * Computes the location on a great circle arc with the given starting
	 * location, azimuth, and arc distance.
	 * 
	 * @param p
	 *            WB_LatLon of the starting location
	 * @param greatCircleAzimuth
	 *            great circle azimuth angle (clockwise from North)
	 * @param pathLength
	 *            arc distance to travel
	 * 
	 * @return WB_LatLon location on the great circle arc.
	 */
	public static WB_LatLon greatCircleEndPosition(WB_LatLon p,
			WB_Angle greatCircleAzimuth, WB_Angle pathLength) {
		if (p == null) {

			throw new IllegalArgumentException();
		}
		if (greatCircleAzimuth == null || pathLength == null) {

			throw new IllegalArgumentException();
		}

		double lat = p.getLatitude().radians;
		double lon = p.getLongitude().radians;
		double azimuth = greatCircleAzimuth.radians;
		double distance = pathLength.radians;

		if (distance == 0)
			return p;

		// Taken from "Map Projections - A Working Manual", page 31, equation
		// 5-5 and 5-6.
		double endLatRadians = Math.asin(Math.sin(lat) * Math.cos(distance)
				+ Math.cos(lat) * Math.sin(distance) * Math.cos(azimuth));
		double endLonRadians = lon
				+ Math.atan2(Math.sin(distance) * Math.sin(azimuth),
						Math.cos(lat) * Math.cos(distance) - Math.sin(lat)
								* Math.sin(distance) * Math.cos(azimuth));

		if (Double.isNaN(endLatRadians) || Double.isNaN(endLonRadians))
			return p;

		return new WB_LatLon(WB_Angle.fromRadians(endLatRadians)
				.normalizedLatitude(), WB_Angle.fromRadians(endLonRadians)
				.normalizedLongitude());
	}

	/**
	 * Computes the location on a great circle arc with the given starting
	 * location, azimuth, and arc distance.
	 * 
	 * @param p
	 *            WB_LatLon of the starting location
	 * @param greatCircleAzimuthRadians
	 *            great circle azimuth angle (clockwise from North), in radians
	 * @param pathLengthRadians
	 *            arc distance to travel, in radians
	 * 
	 * @return WB_LatLon location on the great circle arc.
	 */
	public static WB_LatLon greatCircleEndPosition(WB_LatLon p,
			double greatCircleAzimuthRadians, double pathLengthRadians) {
		if (p == null) {

			throw new IllegalArgumentException();
		}

		return greatCircleEndPosition(p,
				WB_Angle.fromRadians(greatCircleAzimuthRadians),
				WB_Angle.fromRadians(pathLengthRadians));
	}

	/**
	 * Returns two locations with the most extreme latitudes on the great circle
	 * with the given starting location and azimuth.
	 * 
	 * @param location
	 *            location on the great circle.
	 * @param azimuth
	 *            great circle azimuth angle (clockwise from North).
	 * 
	 * @return two locations where the great circle has its extreme latitudes.
	 * 
	 * @throws IllegalArgumentException
	 *             if either <code>location</code> or <code>azimuth</code> are
	 *             null.
	 */
	public static WB_LatLon[] greatCircleExtremeLocations(WB_LatLon location,
			WB_Angle azimuth) {
		if (location == null) {

			throw new IllegalArgumentException();
		}

		if (azimuth == null) {

			throw new IllegalArgumentException();
		}

		double lat0 = location.getLatitude().radians;
		double az = azimuth.radians;

		// Derived by solving the function for longitude on a great circle
		// against the desired longitude. We start with
		// the equation in "Map Projections - A Working Manual", page 31,
		// equation 5-5:
		//
		// lat = asin( sin(lat0) * cos(c) + cos(lat0) * sin(c) * cos(Az) )
		//
		// Where (lat0, lon) are the starting coordinates, c is the angular
		// distance along the great circle from the
		// starting coordinate, and Az is the azimuth. All values are in
		// radians.
		//
		// Solving for angular distance gives distance to the equator:
		//
		// tan(c) = -tan(lat0) / cos(Az)
		//
		// The great circle is by definition centered about the Globe's origin.
		// Therefore intersections with the
		// equator will be antipodal (exactly 180 degrees opposite each other),
		// as will be the extreme latitudes.
		// By observing the symmetry of a great circle, it is also apparent that
		// the extreme latitudes will be 90
		// degrees from either intersection with the equator.
		//
		// d1 = c + 90
		// d2 = c - 90

		double tanDistance = -Math.tan(lat0) / Math.cos(az);
		double distance = Math.atan(tanDistance);

		WB_Angle extremeDistance1 = WB_Angle.fromRadians(distance
				+ (Math.PI / 2.0));
		WB_Angle extremeDistance2 = WB_Angle.fromRadians(distance
				- (Math.PI / 2.0));

		return new WB_LatLon[] {
				greatCircleEndPosition(location, azimuth, extremeDistance1),
				greatCircleEndPosition(location, azimuth, extremeDistance2) };
	}

	/**
	 * Returns two locations with the most extreme latitudes on the great circle
	 * arc defined by, and limited to, the two locations.
	 * 
	 * @param begin
	 *            beginning location on the great circle arc.
	 * @param end
	 *            ending location on the great circle arc.
	 * 
	 * @return two locations with the most extreme latitudes on the great circle
	 *         arc.
	 * 
	 * @throws IllegalArgumentException
	 *             if either <code>begin</code> or <code>end</code> are null.
	 */
	public static WB_LatLon[] greatCircleArcExtremeLocations(WB_LatLon begin,
			WB_LatLon end) {
		if (begin == null) {

			throw new IllegalArgumentException();
		}

		if (end == null) {

			throw new IllegalArgumentException();
		}

		WB_LatLon minLatLocation = null;
		WB_LatLon maxLatLocation = null;
		double minLat = WB_Angle.POS90.degrees;
		double maxLat = WB_Angle.NEG90.degrees;

		// Compute the min and max latitude and associated locations from the
		// arc endpoints.
		for (WB_LatLon ll : java.util.Arrays.asList(begin, end)) {
			if (minLat >= ll.getLatitude().degrees) {
				minLat = ll.getLatitude().degrees;
				minLatLocation = ll;
			}
			if (maxLat <= ll.getLatitude().degrees) {
				maxLat = ll.getLatitude().degrees;
				maxLatLocation = ll;
			}
		}

		// Compute parameters for the great circle arc defined by begin and end.
		// Then compute the locations of extreme
		// latitude on entire the great circle which that arc is part of.
		WB_Angle greatArcAzimuth = greatCircleAzimuth(begin, end);
		WB_Angle greatArcDistance = greatCircleDistance(begin, end);
		WB_LatLon[] greatCircleExtremes = greatCircleExtremeLocations(begin,
				greatArcAzimuth);

		// Determine whether either of the extreme locations are inside the arc
		// defined by begin and end. If so,
		// adjust the min and max latitude accordingly.
		for (WB_LatLon ll : greatCircleExtremes) {
			WB_Angle az = WB_LatLon.greatCircleAzimuth(begin, ll);
			WB_Angle d = WB_LatLon.greatCircleDistance(begin, ll);

			// The extreme location must be between the begin and end locations.
			// Therefore its azimuth relative to
			// the begin location should have the same signum, and its distance
			// relative to the begin location should
			// be between 0 and greatArcDistance, inclusive.
			if (Math.signum(az.degrees) == Math.signum(greatArcAzimuth.degrees)) {
				if (d.degrees >= 0 && d.degrees <= greatArcDistance.degrees) {
					if (minLat >= ll.getLatitude().degrees) {
						minLat = ll.getLatitude().degrees;
						minLatLocation = ll;
					}
					if (maxLat <= ll.getLatitude().degrees) {
						maxLat = ll.getLatitude().degrees;
						maxLatLocation = ll;
					}
				}
			}
		}

		return new WB_LatLon[] { minLatLocation, maxLatLocation };
	}

	/**
	 * Returns two locations with the most extreme latitudes on the sequence of
	 * great circle arcs defined by each pair of locations in the specified
	 * iterable.
	 * 
	 * @param locations
	 *            the pairs of locations defining a sequence of great circle
	 *            arcs.
	 * 
	 * @return two locations with the most extreme latitudes on the great circle
	 *         arcs.
	 * 
	 * @throws IllegalArgumentException
	 *             if <code>locations</code> is null.
	 */
	public static WB_LatLon[] greatCircleArcExtremeLocations(
			Iterable<? extends WB_LatLon> locations) {
		if (locations == null) {
			throw new IllegalArgumentException();
		}

		WB_LatLon minLatLocation = null;
		WB_LatLon maxLatLocation = null;

		WB_LatLon lastLocation = null;

		for (WB_LatLon ll : locations) {
			if (lastLocation != null) {
				WB_LatLon[] extremes = WB_LatLon
						.greatCircleArcExtremeLocations(lastLocation, ll);
				if (extremes == null)
					continue;

				if (minLatLocation == null
						|| minLatLocation.getLatitude().degrees > extremes[0]
								.getLatitude().degrees)
					minLatLocation = extremes[0];
				if (maxLatLocation == null
						|| maxLatLocation.getLatitude().degrees < extremes[1]
								.getLatitude().degrees)
					maxLatLocation = extremes[1];
			}

			lastLocation = ll;
		}

		return new WB_LatLon[] { minLatLocation, maxLatLocation };
	}

	/**
	 * Computes the length of the rhumb line between two locations. The return
	 * value gives the distance as the angular distance between the two
	 * positions on the pi radius circle. In radians, this angle is also the arc
	 * length of the segment between the two positions on that circle. To
	 * compute a distance in meters from this value, multiply it by the radius
	 * of the globe.
	 * 
	 * @param p1
	 *            WB_LatLon of the first location
	 * @param p2
	 *            WB_LatLon of the second location
	 * 
	 * @return the arc length of the rhumb line between the two locations. In
	 *         radians, this value is the arc length on the radius pi circle.
	 */
	public static WB_Angle rhumbDistance(WB_LatLon p1, WB_LatLon p2) {
		if (p1 == null || p2 == null) {
			throw new IllegalArgumentException();
		}

		double lat1 = p1.getLatitude().radians;
		double lon1 = p1.getLongitude().radians;
		double lat2 = p2.getLatitude().radians;
		double lon2 = p2.getLongitude().radians;

		if (lat1 == lat2 && lon1 == lon2)
			return WB_Angle.ZERO;

		// Taken from http://www.movable-type.co.uk/scripts/latlong.html
		double dLat = lat2 - lat1;
		double dLon = lon2 - lon1;
		double dPhi = Math.log(Math.tan(lat2 / 2.0 + Math.PI / 4.0)
				/ Math.tan(lat1 / 2.0 + Math.PI / 4.0));
		double q = dLat / dPhi;
		if (Double.isNaN(dPhi) || Double.isNaN(q)) {
			q = Math.cos(lat1);
		}
		// If lonChange over 180 take shorter rhumb across 180 meridian.
		if (Math.abs(dLon) > Math.PI) {
			dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
		}

		double distanceRadians = Math.sqrt(dLat * dLat + q * q * dLon * dLon);

		return Double.isNaN(distanceRadians) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(distanceRadians);
	}

	/**
	 * Computes the azimuth angle (clockwise from North) of a rhumb line (a line
	 * of constant heading) between two locations.
	 * 
	 * @param p1
	 *            WB_LatLon of the first location
	 * @param p2
	 *            WB_LatLon of the second location
	 * 
	 * @return azimuth WB_Angle of a rhumb line between the two locations.
	 */
	public static WB_Angle rhumbAzimuth(WB_LatLon p1, WB_LatLon p2) {
		if (p1 == null || p2 == null) {

			throw new IllegalArgumentException();
		}

		double lat1 = p1.getLatitude().radians;
		double lon1 = p1.getLongitude().radians;
		double lat2 = p2.getLatitude().radians;
		double lon2 = p2.getLongitude().radians;

		if (lat1 == lat2 && lon1 == lon2)
			return WB_Angle.ZERO;

		// Taken from http://www.movable-type.co.uk/scripts/latlong.html
		double dLon = lon2 - lon1;
		double dPhi = Math.log(Math.tan(lat2 / 2.0 + Math.PI / 4.0)
				/ Math.tan(lat1 / 2.0 + Math.PI / 4.0));
		// If lonChange over 180 take shorter rhumb across 180 meridian.
		if (Math.abs(dLon) > Math.PI) {
			dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
		}
		double azimuthRadians = Math.atan2(dLon, dPhi);

		return Double.isNaN(azimuthRadians) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(azimuthRadians);
	}

	/**
	 * Computes the location on a rhumb line with the given starting location,
	 * rhumb azimuth, and arc distance along the line.
	 * 
	 * @param p
	 *            WB_LatLon of the starting location
	 * @param rhumbAzimuth
	 *            rhumb azimuth angle (clockwise from North)
	 * @param pathLength
	 *            arc distance to travel
	 * 
	 * @return WB_LatLon location on the rhumb line.
	 */
	public static WB_LatLon rhumbEndPosition(WB_LatLon p,
			WB_Angle rhumbAzimuth, WB_Angle pathLength) {
		if (p == null) {

			throw new IllegalArgumentException();
		}
		if (rhumbAzimuth == null || pathLength == null) {

			throw new IllegalArgumentException();
		}

		double lat1 = p.getLatitude().radians;
		double lon1 = p.getLongitude().radians;
		double azimuth = rhumbAzimuth.radians;
		double distance = pathLength.radians;

		if (distance == 0)
			return p;

		// Taken from http://www.movable-type.co.uk/scripts/latlong.html
		double lat2 = lat1 + distance * Math.cos(azimuth);
		double dPhi = Math.log(Math.tan(lat2 / 2.0 + Math.PI / 4.0)
				/ Math.tan(lat1 / 2.0 + Math.PI / 4.0));
		double q = (lat2 - lat1) / dPhi;
		if (Double.isNaN(dPhi) || Double.isNaN(q) || Double.isInfinite(q)) {
			q = Math.cos(lat1);
		}
		double dLon = distance * Math.sin(azimuth) / q;
		// Handle latitude passing over either pole.
		if (Math.abs(lat2) > Math.PI / 2.0) {
			lat2 = lat2 > 0 ? Math.PI - lat2 : -Math.PI - lat2;
		}
		double lon2 = (lon1 + dLon + Math.PI) % (2 * Math.PI) - Math.PI;

		if (Double.isNaN(lat2) || Double.isNaN(lon2))
			return p;

		return new WB_LatLon(WB_Angle.fromRadians(lat2).normalizedLatitude(),
				WB_Angle.fromRadians(lon2).normalizedLongitude());
	}

	/**
	 * Computes the location on a rhumb line with the given starting location,
	 * rhumb azimuth, and arc distance along the line.
	 * 
	 * @param p
	 *            WB_LatLon of the starting location
	 * @param rhumbAzimuthRadians
	 *            rhumb azimuth angle (clockwise from North), in radians
	 * @param pathLengthRadians
	 *            arc distance to travel, in radians
	 * 
	 * @return WB_LatLon location on the rhumb line.
	 */
	public static WB_LatLon rhumbEndPosition(WB_LatLon p,
			double rhumbAzimuthRadians, double pathLengthRadians) {
		if (p == null) {

			throw new IllegalArgumentException();
		}

		return rhumbEndPosition(p, WB_Angle.fromRadians(rhumbAzimuthRadians),
				WB_Angle.fromRadians(pathLengthRadians));
	}

	/**
	 * Computes the length of the linear path between two locations. The return
	 * value gives the distance as the angular distance between the two
	 * positions on the pi radius circle. In radians, this angle is also the arc
	 * length of the segment between the two positions on that circle. To
	 * compute a distance in meters from this value, multiply it by the radius
	 * of the globe.
	 * 
	 * @param p1
	 *            WB_LatLon of the first location
	 * @param p2
	 *            WB_LatLon of the second location
	 * 
	 * @return the arc length of the line between the two locations. In radians,
	 *         this value is the arc length on the radius pi circle.
	 */
	public static WB_Angle linearDistance(WB_LatLon p1, WB_LatLon p2) {
		if (p1 == null || p2 == null) {

			throw new IllegalArgumentException();
		}

		double lat1 = p1.getLatitude().radians;
		double lon1 = p1.getLongitude().radians;
		double lat2 = p2.getLatitude().radians;
		double lon2 = p2.getLongitude().radians;

		if (lat1 == lat2 && lon1 == lon2)
			return WB_Angle.ZERO;

		double dLat = lat2 - lat1;
		double dLon = lon2 - lon1;

		// If lonChange over 180 take shorter path across 180 meridian.
		if (Math.abs(dLon) > Math.PI) {
			dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
		}

		double distanceRadians = Math.hypot(dLat, dLon);

		return Double.isNaN(distanceRadians) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(distanceRadians);
	}

	/**
	 * Computes the azimuth angle (clockwise from North) of a linear path two
	 * locations.
	 * 
	 * @param p1
	 *            WB_LatLon of the first location
	 * @param p2
	 *            WB_LatLon of the second location
	 * 
	 * @return azimuth WB_Angle of a linear path between the two locations.
	 */
	public static WB_Angle linearAzimuth(WB_LatLon p1, WB_LatLon p2) {
		if (p1 == null || p2 == null) {
			throw new IllegalArgumentException();
		}

		double lat1 = p1.getLatitude().radians;
		double lon1 = p1.getLongitude().radians;
		double lat2 = p2.getLatitude().radians;
		double lon2 = p2.getLongitude().radians;

		if (lat1 == lat2 && lon1 == lon2)
			return WB_Angle.ZERO;

		double dLon = lon2 - lon1;
		double dLat = lat2 - lat1;

		// If lonChange over 180 take shorter rhumb across 180 meridian.
		if (Math.abs(dLon) > Math.PI) {
			dLon = dLon > 0 ? -(2 * Math.PI - dLon) : (2 * Math.PI + dLon);
		}
		double azimuthRadians = Math.atan2(dLon, dLat);

		return Double.isNaN(azimuthRadians) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(azimuthRadians);
	}

	/**
	 * Computes the location on a linear path given a starting location,
	 * azimuth, and arc distance along the line. A linear path is determined by
	 * treating latitude and longitude as a rectangular grid. This type of path
	 * is a straight line in the equidistant cylindrical map projection (also
	 * called equirectangular).
	 * 
	 * @param p
	 *            WB_LatLon of the starting location
	 * @param linearAzimuth
	 *            azimuth angle (clockwise from North)
	 * @param pathLength
	 *            arc distance to travel
	 * 
	 * @return WB_LatLon location on the line.
	 */
	public static WB_LatLon linearEndPosition(WB_LatLon p,
			WB_Angle linearAzimuth, WB_Angle pathLength) {
		if (p == null) {

			throw new IllegalArgumentException();
		}
		if (linearAzimuth == null || pathLength == null) {

			throw new IllegalArgumentException();
		}

		double lat1 = p.getLatitude().radians;
		double lon1 = p.getLongitude().radians;
		double azimuth = linearAzimuth.radians;
		double distance = pathLength.radians;

		if (distance == 0)
			return p;

		double lat2 = lat1 + distance * Math.cos(azimuth);

		// Handle latitude passing over either pole.
		if (Math.abs(lat2) > Math.PI / 2.0) {
			lat2 = lat2 > 0 ? Math.PI - lat2 : -Math.PI - lat2;
		}
		double lon2 = (lon1 + distance * Math.sin(azimuth) + Math.PI)
				% (2 * Math.PI) - Math.PI;

		if (Double.isNaN(lat2) || Double.isNaN(lon2))
			return p;

		return new WB_LatLon(WB_Angle.fromRadians(lat2).normalizedLatitude(),
				WB_Angle.fromRadians(lon2).normalizedLongitude());
	}

	/**
	 * Compute the average rhumb distance between locations.
	 * 
	 * @param locations
	 *            Locations of which to compute average.
	 * 
	 * @return Average rhumb line distance between locations, as an angular
	 *         distance.
	 */
	public static WB_Angle getAverageDistance(
			Iterable<? extends WB_LatLon> locations) {
		if ((locations == null)) {

			throw new IllegalArgumentException();
		}

		double totalDistance = 0.0;
		int count = 0;

		for (WB_LatLon p1 : locations) {
			for (WB_LatLon p2 : locations) {
				if (p1 != p2) {
					double d = rhumbDistance(p1, p2).radians;
					totalDistance += d;
					count++;
				}
			}
		}

		return (count == 0) ? WB_Angle.ZERO : WB_Angle
				.fromRadians(totalDistance / (double) count);
	}

	public WB_LatLon add(WB_LatLon that) {
		if (that == null) {

			throw new IllegalArgumentException();
		}

		WB_Angle lat = WB_Angle.normalizedLatitude(this.latitude
				.add(that.latitude));
		WB_Angle lon = WB_Angle.normalizedLongitude(this.longitude
				.add(that.longitude));

		return new WB_LatLon(lat, lon);
	}

	public WB_LatLon subtract(WB_LatLon that) {
		if (that == null) {

			throw new IllegalArgumentException();
		}

		WB_Angle lat = WB_Angle.normalizedLatitude(this.latitude
				.subtract(that.latitude));
		WB_Angle lon = WB_Angle.normalizedLongitude(this.longitude
				.subtract(that.longitude));

		return new WB_LatLon(lat, lon);
	}

	public WB_LatLon add(WB_SphericalPosition that) {
		if (that == null) {

			throw new IllegalArgumentException();
		}

		WB_Angle lat = WB_Angle.normalizedLatitude(this.latitude.add(that
				.getLatitude()));
		WB_Angle lon = WB_Angle.normalizedLongitude(this.longitude.add(that
				.getLongitude()));

		return new WB_LatLon(lat, lon);
	}

	public WB_LatLon subtract(WB_SphericalPosition that) {
		if (that == null) {

			throw new IllegalArgumentException();
		}

		WB_Angle lat = WB_Angle.normalizedLatitude(this.latitude.subtract(that
				.getLatitude()));
		WB_Angle lon = WB_Angle.normalizedLongitude(this.longitude
				.subtract(that.getLongitude()));

		return new WB_LatLon(lat, lon);
	}

	public static boolean locationsCrossDateLine(
			Iterable<? extends WB_LatLon> locations) {
		if (locations == null) {

			throw new IllegalArgumentException();
		}

		WB_LatLon pos = null;
		for (WB_LatLon posNext : locations) {
			if (pos != null) {
				// A segment cross the line if end pos have different longitude
				// signs
				// and are more than 180 degrees longitude apart
				if (Math.signum(pos.getLongitude().degrees) != Math
						.signum(posNext.getLongitude().degrees)) {
					double delta = Math.abs(pos.getLongitude().degrees
							- posNext.getLongitude().degrees);
					if (delta > 180 && delta < 360)
						return true;
				}
			}
			pos = posNext;
		}

		return false;
	}

	public static boolean locationsCrossDateline(WB_LatLon p1, WB_LatLon p2) {
		if (p1 == null || p2 == null) {

			throw new IllegalArgumentException();
		}

		// A segment cross the line if end pos have different longitude signs
		// and are more than 180 degrees longitude apart
		if (Math.signum(p1.getLongitude().degrees) != Math.signum(p2
				.getLongitude().degrees)) {
			double delta = Math.abs(p1.getLongitude().degrees
					- p2.getLongitude().degrees);
			if (delta > 180 && delta < 360)
				return true;
		}

		return false;
	}

	/**
	 * Transform the negative longitudes of a dateline-spanning location list to
	 * positive values that maintain the relationship with the other locations
	 * in the list. Negative longitudes are transformed to values greater than
	 * 180 degrees, as though longitude spanned [0, 360] rather than [-180,
	 * 180]. This enables arithmetic operations to be performed on the locations
	 * without having to take into account the longitude jump at the dateline.
	 * 
	 * @param locations
	 *            the locations to transform. This list is not modified.
	 * 
	 * @return a new list of locations transformed as described above.
	 * 
	 * @throws IllegalArgumentException
	 *             if the location list is null.
	 */
	public static List<WB_LatLon> makeDatelineCrossingLocationsPositive(
			Iterable<? extends WB_LatLon> locations) {
		if (locations == null) {

			throw new IllegalArgumentException();
		}

		Iterator iter = locations.iterator();
		if (!iter.hasNext())
			return Collections.emptyList();

		ArrayList<WB_LatLon> newLocations = new ArrayList<WB_LatLon>();

		for (WB_LatLon location : locations) {
			if (location == null)
				continue;

			if (location.getLongitude().degrees < 0) {
				newLocations.add(WB_LatLon.fromDegrees(
						location.getLatitude().degrees,
						location.getLongitude().degrees + 360));
			} else {
				newLocations.add(location);
			}
		}

		return newLocations;
	}

	@Override
	public String toString() {
		String las = String.format("Lat %7.4f\u00B0", this.getLatitude()
				.getDegrees());
		String los = String.format("Lon %7.4f\u00B0", this.getLongitude()
				.getDegrees());
		return "(" + las + ", " + los + ")";
	}

	@Override
	public boolean equals(Object o) {
		if (this == o)
			return true;
		if (o == null || getClass() != o.getClass())
			return false;

		final WB_LatLon latLon = (WB_LatLon) o;

		if (!latitude.equals(latLon.latitude))
			return false;
		// noinspection RedundantIfStatement
		if (!longitude.equals(latLon.longitude))
			return false;

		return true;
	}

	public static boolean equals(WB_LatLon a, WB_LatLon b) {
		return a.getLatitude().equals(b.getLatitude())
				&& a.getLongitude().equals(b.getLongitude());
	}

	@Override
	public int hashCode() {
		int result;
		result = latitude.hashCode();
		result = 29 * result + longitude.hashCode();
		return result;
	}

	/**
	 * Compute the forward azimuth between two positions
	 * 
	 * @param p1
	 *            first position
	 * @param p2
	 *            second position
	 * @param equatorialRadius
	 *            the equatorial radius of the globe in meters
	 * @param polarRadius
	 *            the polar radius of the globe in meters
	 * 
	 * @return the azimuth
	 */
	public static WB_Angle ellipsoidalForwardAzimuth(WB_LatLon p1,
			WB_LatLon p2, double equatorialRadius, double polarRadius) {
		if (p1 == null || p2 == null) {
			throw new IllegalArgumentException();
		}

		// Calculate flattening
		final double f = (equatorialRadius - polarRadius) / equatorialRadius; // flattening

		// Calculate reduced latitudes and related sines/cosines
		final double U1 = Math.atan((1.0 - f) * Math.tan(p1.latitude.radians));
		final double cU1 = Math.cos(U1);
		final double sU1 = Math.sin(U1);

		final double U2 = Math.atan((1.0 - f) * Math.tan(p2.latitude.radians));
		final double cU2 = Math.cos(U2);
		final double sU2 = Math.sin(U2);

		// Calculate difference in longitude
		final double L = p2.longitude.subtract(p1.longitude).radians;

		// Vincenty's Formula for Forward Azimuth
		// iterate until change in lambda is negligible (e.g. 1e-12 ~= 0.06mm)
		// first approximation
		double lambda = L;
		double sLambda = Math.sin(lambda);
		double cLambda = Math.cos(lambda);

		// dummy value to ensure
		double lambda_prev = Double.MAX_VALUE;
		int count = 0;
		while (Math.abs(lambda - lambda_prev) > 1e-12 && count++ < 100) {
			// Store old lambda
			lambda_prev = lambda;
			// Calculate new lambda
			double sSigma = Math.sqrt(Math.pow(cU2 * sLambda, 2)
					+ Math.pow(cU1 * sU2 - sU1 * cU2 * cLambda, 2));
			double cSigma = sU1 * sU2 + cU1 * cU2 * cLambda;
			double sigma = Math.atan2(sSigma, cSigma);
			double sAlpha = cU1 * cU2 * sLambda / sSigma;
			double cAlpha2 = 1 - sAlpha * sAlpha; // trig identity
			// As cAlpha2 approaches zeros, set cSigmam2 to zero to converge on
			// a solution
			double cSigmam2;
			if (Math.abs(cAlpha2) < 1e-6) {
				cSigmam2 = 0;
			} else {
				cSigmam2 = cSigma - 2 * sU1 * sU2 / cAlpha2;
			}
			double c = f / 16 * cAlpha2 * (4 + f * (4 - 3 * cAlpha2));

			lambda = L
					+ (1 - c)
					* f
					* sAlpha
					* (sigma + c * sSigma
							* (cSigmam2 + c * cSigma * (-1 + 2 * cSigmam2)));
			sLambda = Math.sin(lambda);
			cLambda = Math.cos(lambda);
		}

		return WB_Angle.fromRadians(Math.atan2(cU2 * sLambda, cU1 * sU2 - sU1
				* cU2 * cLambda));
	}

	/**
	 * Computes the distance between two points on an ellipsoid iteratively.
	 * <p/>
	 * NOTE: This method was copied from the UniData NetCDF Java library.
	 * http://www.unidata.ucar.edu/software/netcdf-java/
	 * <p/>
	 * Algorithm from U.S. National Geodetic Survey, FORTRAN program "inverse,"
	 * subroutine "INVER1," by L. PFEIFER and JOHN G. GERGEN. See
	 * http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
	 * <p/>
	 * Original documentation: SOLUTION OF THE GEODETIC INVERSE PROBLEM AFTER
	 * T.VINCENTY MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
	 * EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
	 * STANDPOINT/FOREPOINT MUST NOT BE THE GEOGRAPHIC POLE
	 * <p/>
	 * 
	 * @param p1
	 *            first position
	 * @param p2
	 *            second position
	 * @param equatorialRadius
	 *            the equatorial radius of the globe in meters
	 * @param polarRadius
	 *            the polar radius of the globe in meters
	 * 
	 * @return distance in meters between the two points
	 */
	public static double ellipsoidalDistance(WB_LatLon p1, WB_LatLon p2,
			double equatorialRadius, double polarRadius) {

		final double F = (equatorialRadius - polarRadius) / equatorialRadius; // flattening
																				// =
																				// 1.0
																				// /
																				// 298.257223563;
		final double R = 1.0 - F;
		final double EPS = 0.5E-13;

		if (p1 == null || p2 == null) {
			throw new IllegalArgumentException();
		}

		// Algorithm from National Geodetic Survey, FORTRAN program "inverse,"
		// subroutine "INVER1," by L. PFEIFER and JOHN G. GERGEN.
		// http://www.ngs.noaa.gov/TOOLS/Inv_Fwd/Inv_Fwd.html
		// Conversion to JAVA from FORTRAN was made with as few changes as
		// possible
		// to avoid errors made while recasting form, and to facilitate any
		// future
		// comparisons between the original code and the altered version in
		// Java.
		// Original documentation:
		// SOLUTION OF THE GEODETIC INVERSE PROBLEM AFTER T.VINCENTY
		// MODIFIED RAINSFORD'S METHOD WITH HELMERT'S ELLIPTICAL TERMS
		// EFFECTIVE IN ANY AZIMUTH AND AT ANY DISTANCE SHORT OF ANTIPODAL
		// STANDPOINT/FOREPOINT MUST NOT BE THE GEOGRAPHIC POLE
		// A IS THE SEMI-MAJOR AXIS OF THE REFERENCE ELLIPSOID
		// F IS THE FLATTENING (NOT RECIPROCAL) OF THE REFERNECE ELLIPSOID
		// LATITUDES GLAT1 AND GLAT2
		// AND LONGITUDES GLON1 AND GLON2 ARE IN RADIANS POSITIVE NORTH AND EAST
		// FORWARD AZIMUTHS AT BOTH POINTS RETURNED IN RADIANS FROM NORTH
		//
		// Reference ellipsoid is the WGS-84 ellipsoid.
		// See http://www.colorado.edu/geography/gcraft/notes/datum/elist.html
		// FAZ is forward azimuth in radians from pt1 to pt2;
		// BAZ is backward azimuth from point 2 to 1;
		// S is distance in meters.
		//
		// Conversion to JAVA from FORTRAN was made with as few changes as
		// possible
		// to avoid errors made while recasting form, and to facilitate any
		// future
		// comparisons between the original code and the altered version in
		// Java.
		//
		// IMPLICIT REAL*8 (A-H,O-Z)
		// COMMON/CONST/PI,RAD

		double GLAT1 = p1.getLatitude().radians;
		double GLAT2 = p2.getLatitude().radians;
		double TU1 = R * Math.sin(GLAT1) / Math.cos(GLAT1);
		double TU2 = R * Math.sin(GLAT2) / Math.cos(GLAT2);
		double CU1 = 1. / Math.sqrt(TU1 * TU1 + 1.);
		double SU1 = CU1 * TU1;
		double CU2 = 1. / Math.sqrt(TU2 * TU2 + 1.);
		double S = CU1 * CU2;
		double BAZ = S * TU2;
		double FAZ = BAZ * TU1;
		double GLON1 = p1.getLongitude().radians;
		double GLON2 = p2.getLongitude().radians;
		double X = GLON2 - GLON1;
		double D, SX, CX, SY, CY, Y, SA, C2A, CZ, E, C;
		do {
			SX = Math.sin(X);
			CX = Math.cos(X);
			TU1 = CU2 * SX;
			TU2 = BAZ - SU1 * CU2 * CX;
			SY = Math.sqrt(TU1 * TU1 + TU2 * TU2);
			CY = S * CX + FAZ;
			Y = Math.atan2(SY, CY);
			SA = S * SX / SY;
			C2A = -SA * SA + 1.;
			CZ = FAZ + FAZ;
			if (C2A > 0.) {
				CZ = -CZ / C2A + CY;
			}
			E = CZ * CZ * 2. - 1.;
			C = ((-3. * C2A + 4.) * F + 4.) * C2A * F / 16.;
			D = X;
			X = ((E * CY * C + CZ) * SY * C + Y) * SA;
			X = (1. - C) * X * F + GLON2 - GLON1;
			// IF(DABS(D-X).GT.EPS) GO TO 100
		} while (Math.abs(D - X) > EPS);

		// FAZ = Math.atan2(TU1, TU2);
		// BAZ = Math.atan2(CU1 * SX, BAZ * CX - SU1 * CU2) + Math.PI;
		X = Math.sqrt((1. / R / R - 1.) * C2A + 1.) + 1.;
		X = (X - 2.) / X;
		C = 1. - X;
		C = (X * X / 4. + 1.) / C;
		D = (0.375 * X * X - 1.) * X;
		X = E * CY;
		S = 1. - E - E;
		S = ((((SY * SY * 4. - 3.) * S * CZ * D / 6. - X) * D / 4. + CZ) * SY
				* D + Y)
				* C * equatorialRadius * R;

		return S;
	}

}
